#include "ControlLegUpdown.h"

using namespace gazebo;
using namespace std;

void ControlLegUpdown::Load(physics::WorldPtr world)
{
  //获取模型
  this->model_leg = world->GetModel("spring_leg");
  //获取spring_leg中的spring_joint部分
  this->spring_joint = this->model_leg->GetJoint("spring_joint");
  //获取spring_leg中的leg_low部分
  this->leg_low_link = this->model_leg->GetChildLink("leg_low");
  //获取spring_leg中的leg_high部分
  this->leg_high_link = this->model_leg->GetChildLink("leg_high");

  //初始化Spring_Leg
  //这里只给P参数，用来模拟f = k × deltaX
  //期望应该为leg_high的长度
  Spring_Leg.PID_init(SPRING_K,0.0,0.0,axis_spring_leg_low);
}

 //弹跳高度控制函数
void ControlLegUpdown::Control_Height(double Height ,double Mass_Leg ,double Spring_K)
{
	//获得delta_x
	//求delta_x的方法就是获取从fixed_joint指向spring_joint的向量
	double delta_x = 0.0;
	double force_spring = 0.0,force_compensate = 0.0;

	math::Vector3 Position_leg_low_link;
	math::Vector3 Position_leg_high_link;
	math::Vector3 delta_position;
	math::Vector3 LinearVel_leg_high;
	math::Vector3 LinearVel_leg_low;

	double Length_leg_high;
	double Length_leg_low;

	//获取leg_high和leg_low的长度
	Length_leg_high = ((math::Vector3)(leg_high_link->GetInitialRelativePose().pos)).z;
	Length_leg_low = ((math::Vector3)(leg_low_link->GetInitialRelativePose().pos)).z;

	//获取关节位置
	delta_position = spring_joint->GetAnchorErrorPose().pos;

	//计算弹力,输出
	delta_x = delta_position.z;
	force_spring = -Spring_Leg.PID_calc(delta_x);

	//特此感谢薛泽文同志、程鑫同志、曹阳同志给予的帮助
	//这里补偿力应该是从压缩到最底部开始，速度为正且接触地面的时候
	//施加的一段时间的力
	//
	//获取leg_high_link的速度为了判断最低点
	LinearVel_leg_high = leg_high_link->GetRelativeLinearVel();
	//获取leg_low_link的绝对z轴位置，判断是否接触地面
	Position_leg_low_link = leg_low_link->GetWorldPose().pos;
    Position_leg_high_link = leg_high_link->GetWorldPose().pos;

	if(LinearVel_leg_high.z > 0 && fabs(Position_leg_low_link.z - Length_leg_low) < 0.001)
	{
	    //此时吸气->给力
	    //F = (mgh - k * delta_x^2 / 2)/delta_x
	    //实际上应该是J + J上升损失的能量 + 1/2（kx^2）= mgh
	    //但是我们算的时J = mgh - 1/2(kx^2)
	    //其中J只占2/3
	    //所以真正要补偿的能量时J + J上升损失的能量 = J * 3/2
	    //损失1/3
	    //注意：损失的能量比例是应该变的，这里空气阻力应该时非线性的，只是做了近似线性处理
	    //另外还有两边柱子的摩擦力因素，所以现在是33%的能量损失
	    //这里取消了阻力，delta_x应该是leg_high与地面的距离 - 0.25
	    force_compensate = -(Mass_Leg * Gravity * Height - (Spring_K * pow(delta_x - axis_spring_leg_low,2))/2) / (fabs(Position_leg_high_link.z - Length_leg_low) * 1);
	    if(force_compensate > 0)
	    {
		    force_compensate = 0;
	    }
    }
	spring_joint->SetForce(0,force_spring + force_compensate);

	if(fabs(LinearVel_leg_high.z) < 0.01 )
	{
	    printf("leg_high_link pos: %lf\r\n",(leg_high_link->GetWorldPose().pos.z));
	    //printf("leg_low_link pos: %lf\r\n",(leg_low_link->GetWorldPose().pos.z) - 0.25);
	}
}


